from utils import *
import sympy as sp
import numpy as np
import copy
    
def next_wp_simple(ps, pt, p1, v1):
    '''ps: self pose, pt: target pose,
    p1: obstacle pose, v1: obstacle speed'''
    vc=0.5
    p1_trans=trans_relative_co(ps, p1)
    if p1_trans.get_norm()<0.7:
        logging.debug(f'see obstacles')
        if p1_trans.y<0:
            return pose(0, vc, 0)
        else:
            return pose(0,-vc,0)
    else:
        logging.debug('no obstacles')
        pt_trans=trans_relative_co(ps, pt)
        phi_pt=limit_pi(pt.phi-ps.phi)
        return pose(pt_trans.x, pt_trans.y, phi_pt)  
    
def next_wp_static(ps, pt, p1, v1):
    '''ps: self pose, pt: target pose,
    p1: obstacle pose, v1: obstacle speed'''
    d=0.5
    x,y=sp.symbols('x, y')
    # print(f'self:{ps} obstacle: {p1}')
    solutions=sp.nonlinsolve([(p1.x-x)*(ps.x-pt.x)+(p1.y-y)*(ps.y-pt.y),
                              (x-p1.x)**2+(y-p1.y)**2-d**2],[x,y])
    ptf=pose(0,0,180)
    for s in solutions:
        tn=np.array([s[0],s[1]])
        tt=np.array([pt.x, pt.y])
        t1=np.array([p1.x, p1.y])
        ts=np.array([ps.x, ps.y])
        if np.cross(tt-ts, tn-ts)*np.cross(tt-ts, t1-ts)<=0:
            print(f'解：{s[0]},{s[1]}')
            ptf=pose(s[0], s[1], 180)
            break
    pt_trans=trans_relative_co(ps, ptf)
    phi_pt=limit_pi(ptf.phi-ps.phi)
    pt_trans=pose(pt_trans.x, pt_trans.y, phi_pt)
    return pt_trans

def next_wp(ps, pt, p1, v1):
    '''ps: self pose, pt: target pose,
    p1: obstacle pose, v1: obstacle speed'''
    d=0.7
    x,y=sp.symbols('x, y')
    # print(f'self:{ps} obstacle: {p1}')
    solutions=sp.nonlinsolve([(p1.x-x)*(ps.x-pt.x)+(p1.y-y)*(ps.y-pt.y),
                              (x-p1.x)**2+(y-p1.y)**2-d**2],[x,y])
    ptf=copy.deepcopy(pt) # 修正的pt
    for s in solutions:
        tn=np.array([s[0],s[1]])
        tt=np.array([pt.x, pt.y])
        t1=np.array([p1.x, p1.y])
        ts=np.array([ps.x, ps.y])
        if np.cross(tt-ts, tn-ts)*np.cross(tt-ts, t1-ts)<=0:
            print(f'解：{s[0]},{s[1]}')
            ptf=pose(s[0], s[1], pt.phi)
            break
    pt_trans=trans_relative_co(ps, ptf)
    phi_pt=limit_pi(ptf.phi-ps.phi)
    pt_trans=pose(pt_trans.x, pt_trans.y, phi_pt)
    return pt_trans